#ifndef COLLIDE_H
#define COLLIDE_H

#include <iostream>
#include <fstream>
#include <vector>

#define OZCOLLIDE_PCH
#include "ozcollide/ozcollide.h"

#include "GL/glut.h"
#include "utility.h"
#include "mesh.h"

class Collide
{
public:
	//Build AABBTree here
	Collide (Mesh * mesh): mesh(mesh)
	{
		for(unsigned i = 0; i < mesh->vertices.size(); i++)//reading vertices
		{
			//Add vertex positions
			Vertex * v = mesh->vertices[i]; 
			ozcollide::Vec3f tmpV(v->point.x,v->point.y,v->point.z);
			verts.add(tmpV);//adding each readed vertex to the list
		}
		for(unsigned i = 0; i < mesh->v_faces.size(); ++i)
		{

			//Add normals
			Vector3 normal = mesh->v_faces[i]->normal();
			normal.normalize(); 
			ozcollide::Vec3f tmpN(normal.x,normal.y,normal.z);
			normals.add(tmpN);//adding each readed normal to the list(one per face)

			//Add faces
			ozcollide::Polygon *p = new ozcollide::Polygon();//we must build each face before add it to the list
			unsigned *indx = new unsigned[3];
			ozcollide::Vec3f tmpNormal(normals[i]);
			mesh->v_faces[i]->Face2VertexIdx(indx);
			indices.add(indx[0]);
			indices.add(indx[1]);
			indices.add(indx[2]);
			p->setIndicesMemory(3,(int *)&indx[0]);//setting up indices
			p->setNormal(tmpNormal);//adding normals (previously readed)
			tris.add(* p);//adding the polygon to the polygon list
		}

		ozcollide::AABBTreePolyBuilder builder;
		BaseTree = builder.buildFromPolys(tris.mem(),//polygons
			tris.size(),//polygon count
			verts.mem(),//vertices
			verts.size());//vertices count
	}
	void segmentCollide (Point3 p1, Point3 p2, std::vector<Face * >& faces)
	{
		//Do collision
		BaseTree->collideWithSegment(ozcollide::Vec3f(p1.x,p1.y,p1.z), 
			ozcollide::Vec3f(p2.x,p2.y,p2.z), segmentResult);
		//Get the faces 

		for(int i = 0; i < segmentResult.polys_.size(); i++)
		{
			unsigned idx[3];
			for(int j = 0; j < 3; j++)
			{
				idx[j] = segmentResult.polys_[i]->getIndex(j) ;
				
				//std::cout<<" ("<<segmentResult.polys_[i]->getIndex(j)<< ") " ;
				//ozcollide::Vec3f tmpVec = BaseTree->getPointsList()[segmentResult.polys_[i]->getIndex(j)];
				//std::cout<<tmpVec.x<<' '<<tmpVec.y<<' '<<tmpVec.z<<std::endl;
			}
			faces.push_back(mesh->faces[faceHashKey(idx[0], idx[1], idx[2])]);
		}
	}
	~ Collide ()
	{
		verts.clear();
		tris.clear();
	}
private:

	Mesh * mesh; 
	ozcollide::AABBTreePoly * BaseTree; 
	ozcollide::Vector<ozcollide::Vec3f> verts,normals;
	ozcollide::Vector<ozcollide::Polygon> tris;
	ozcollide::Vector<int> indices; 
	ozcollide::AABBTreePoly::SegmentColResult segmentResult ;

};


#endif